
#include <cassert>

namespace g3
{

// range_iterator<T>

template <typename T>
range_iterator<T>::range_iterator() :
	_powner(0),
	_box(0, 0, 0, 0, 0, 0),
	_pos(0, 0, 0),
	_pbucket(0)
{
}

template <typename T>
range_iterator<T>::range_iterator(const hash_grid<T> & owner, const box & b) :
	_powner(&owner),
	_box(0, 0, 0, 0, 0, 0),
	_pos(0, 0, 0),
	_pbucket(0)
{
	vectori min;
	vectori max;

	min.x = int(floor(b.x1 / _powner->_cell));
	min.y = int(floor(b.y1 / _powner->_cell));
	min.z = int(floor(b.z1 / _powner->_cell));
	max.x = int(floor(b.x2 / _powner->_cell)) + 1;
	max.y = int(floor(b.y2 / _powner->_cell)) + 1;
	max.z = int(floor(b.z2 / _powner->_cell)) + 1;

	_box = boxi(min, max);
	_pbucket = 0;

	// find first non-empty bucket
	for (_pos.z = min.z; _pos.z != max.z; ++_pos.z)
		for (_pos.y = min.y; _pos.y != max.y; ++_pos.y)
			for (_pos.x = min.x; _pos.x != max.x; ++_pos.x)
			{
				_pbucket = _powner->get_bucket(_pos);
				if (_pbucket != 0)
				{
					_it = _pbucket->begin();
					return;
				}
			}
}

template <typename T>
range_iterator<T>::range_iterator(const range_iterator & rhs) :
	_powner(rhs._powner),
	_box(rhs._box),
	_pos(rhs._pos),
	_pbucket(rhs._pbucket),
	_it(rhs._it)
{
}

template <typename T>
bool range_iterator<T>::end() const
{
	return (_pbucket == 0 || _it == _pbucket->end());
}

template <typename T>
range_iterator<T> & range_iterator<T>::operator = (const range_iterator & rhs)
{
	if (rhs.end())
	{
		_pbucket = 0;
	}
	else
	{
		_powner = rhs._powner;
		_box = rhs._box;
		_pos = rhs._pos;
		_pbucket = rhs._pbucket;
		_it = rhs._it;
	}
	return * this;
}

template <typename T>
bool range_iterator<T>::operator == (const range_iterator & rhs) const
{
	if (rhs.end())
		return end();
	else
		return (_it == rhs._it);
}

template <typename T>
bool range_iterator<T>::operator != (const range_iterator & rhs) const
{
	return !operator == (rhs);
}

template <typename T>
range_iterator<T> & range_iterator<T>::operator ++ ()
{
	assert(!end());
	++ _it;
	// If we have finished current bucket, get the next one...
	while (_pbucket == 0 || _it == _pbucket->end())
	{
		++_pos.x;
		if (_pos.x == _box.x2)
		{
			++_pos.y;
			if (_pos.y == _box.y2)
			{
				++_pos.z;
				if (_pos.z == _box.z2)
				{
					// We have finished the box, so set the 'end' to true
					_pbucket = 0;
					return * this;
				}
				_pos.y = _box.y1;
			}
			_pos.x = _box.x1;
		}
		_pbucket = _powner->get_bucket(_pos);
		if (_pbucket != 0)
			_it = _pbucket->begin();
	}
	return * this;
}

template <typename T>
const T & range_iterator<T>::operator * () const
{
	assert(!end());
	return * _it;
}

// hash_grid<T>

template <typename T>
hash_grid<T>::hash_grid() :
	_cell(1.0)
{

}

template <typename T>
void hash_grid<T>::reset(double c)
{
	_hash.clear();
	_cell = c;
}

template <typename T>
void hash_grid<T>::insert(const box & b, const T & value)
{
	vectori min;
	vectori max;
	vectori pos;

	min.x = int(floor(b.x1 / _cell));
	min.y = int(floor(b.y1 / _cell));
	min.z = int(floor(b.z1 / _cell));
	max.x = int(floor(b.x2 / _cell)) + 1;
	max.y = int(floor(b.y2 / _cell)) + 1;
	max.z = int(floor(b.z2 / _cell)) + 1;

	for (pos.z = min.z; pos.z != max.z; ++pos.z)
		for (pos.y = min.y; pos.y != max.y; ++pos.y)
			for (pos.x = min.x; pos.x != max.x; ++pos.x)
				create_bucket(pos)->insert(value);
}

template <typename T>
typename hash_grid<T>::range_iterator hash_grid<T>::range_begin(const box & b) const
{
	return range_iterator(* this, b);
}

template <typename T>
typename hash_grid<T>::range_iterator hash_grid<T>::range_end() const
{
	return range_iterator();
}

template <typename T>
const typename hash_grid<T>::bucket * hash_grid<T>::get_bucket(const vectori & pos) const
{
/*
	size_t index = abs(11 * pos.x + 73 * pos.y + 113 * pos.z) % d_Table.size();
	const std::list<ListEntry> & list = d_Table[index];
	// Search the list
	std::list<ListEntry>::const_iterator it;
	for (it = list.begin(); it != list.end(); ++it)
		if (it->first == pos)
			return &it->second;
*/
	hash_map::const_iterator it = _hash.lower_bound(pos);
	if (it == _hash.end() || it->first != pos)
		return 0;
	else
		return & it->second;
}

template <typename T>
typename hash_grid<T>::bucket * hash_grid<T>::create_bucket(const vectori & pos)
{
/*
	size_t index = abs(11 * pos.x + 73 * pos.y + 113 * pos.z) % d_Table.size();
	std::list<ListEntry> & list = d_Table[index];
	// Search the list
	std::list<ListEntry>::iterator it;
	for (it = list.begin(); it != list.end(); ++it)
		if (it->first == pos)
			return &it->second;
*/
	// If not found, create and return a new bucket 
/*
	list.push_back(ListEntry(pos, Bucket()));
	return &list.back().second;
*/
	return & _hash[pos];
}

} // g3
